{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [],
   "source": [
    "%matplotlib inline\n",
    "from IPython.display import Markdown, display\n",
    "import numpy as np\n",
    "import matplotlib.dates as mdates\n",
    "import matplotlib.pyplot as plt\n",
    "import time\n",
    "from scipy.stats import norm\n",
    "from sympy import Symbol, symbols, Matrix, sin, cos, integrate, diff, Q, refine, simplify, factor, expand_trig, exp, latex, Integral\n",
    "from sympy import init_printing\n",
    "from sympy.utilities.codegen import codegen\n",
    "init_printing(use_latex=True)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Motion models\n",
    "In tracking we can use different motion models. Here, we will derive a couple of those:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## CATR - constant acceleration and turn rate model\n",
    "This model assumes the state $[x, y, \\theta, v, \\omega, a]^\\top$, where $x$ and $y$ are coordinates of the object, $\\theta$ is it CCW orientation, $v$ is its speed along the vector spawned by $\\theta$, $\\omega$ is the angular velocity (aka turn rate), and $a$ is the acceleration along the vector spawned by $\\theta$. Furthermore, we assume that the object has constant acceleration $a$ and constant turn rate $\\omega$. This is a useful model to model differential-drive systems.\n",
    "\n",
    "### Derivation:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {
    "scrolled": false
   },
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "CART state is:\n"
     ]
    },
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\left[\\begin{matrix}x\\\\y\\\\\\theta\\\\v\\\\\\omega\\\\a\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "⎡  x   ⎤\n",
       "⎢      ⎥\n",
       "⎢  y   ⎥\n",
       "⎢      ⎥\n",
       "⎢\\theta⎥\n",
       "⎢      ⎥\n",
       "⎢  v   ⎥\n",
       "⎢      ⎥\n",
       "⎢\\omega⎥\n",
       "⎢      ⎥\n",
       "⎣  a   ⎦"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Assuming a non-zero turn rate, the next state is:\n"
     ]
    },
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\left[\\begin{matrix}\\frac{\\Delta{t} a \\sin{\\left(\\Delta{t} \\hat{\\omega} + \\theta \\right)}}{\\hat{\\omega}} + x - \\frac{v \\sin{\\left(\\theta \\right)}}{\\hat{\\omega}} + \\frac{v \\sin{\\left(\\Delta{t} \\hat{\\omega} + \\theta \\right)}}{\\hat{\\omega}} - \\frac{a \\cos{\\left(\\theta \\right)}}{\\hat{\\omega}^{2}} + \\frac{a \\cos{\\left(\\Delta{t} \\hat{\\omega} + \\theta \\right)}}{\\hat{\\omega}^{2}}\\\\- \\frac{\\Delta{t} a \\cos{\\left(\\Delta{t} \\hat{\\omega} + \\theta \\right)}}{\\hat{\\omega}} + y + \\frac{v \\cos{\\left(\\theta \\right)}}{\\hat{\\omega}} - \\frac{v \\cos{\\left(\\Delta{t} \\hat{\\omega} + \\theta \\right)}}{\\hat{\\omega}} - \\frac{a \\sin{\\left(\\theta \\right)}}{\\hat{\\omega}^{2}} + \\frac{a \\sin{\\left(\\Delta{t} \\hat{\\omega} + \\theta \\right)}}{\\hat{\\omega}^{2}}\\\\\\Delta{t} \\hat{\\omega} + \\theta\\\\\\Delta{t} a + v\\\\\\hat{\\omega}\\\\a\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "⎡ \\Delta{t}⋅a⋅sin(\\Delta{t}⋅\\hat{\\omega} + \\theta)       v⋅sin(\\theta)   v⋅sin\n",
       "⎢ ──────────────────────────────────────────────── + x - ───────────── + ─────\n",
       "⎢                   \\hat{\\omega}                          \\hat{\\omega}        \n",
       "⎢                                                                             \n",
       "⎢                                                                             \n",
       "⎢  \\Delta{t}⋅a⋅cos(\\Delta{t}⋅\\hat{\\omega} + \\theta)       v⋅cos(\\theta)   v⋅co\n",
       "⎢- ──────────────────────────────────────────────── + y + ───────────── - ────\n",
       "⎢                    \\hat{\\omega}                          \\hat{\\omega}       \n",
       "⎢                                                                             \n",
       "⎢                                                                             \n",
       "⎢                                                                    \\Delta{t}\n",
       "⎢                                                                             \n",
       "⎢                                                                            \\\n",
       "⎢                                                                             \n",
       "⎢                                                                             \n",
       "⎢                                                                             \n",
       "⎣                                                                             \n",
       "\n",
       "(\\Delta{t}⋅\\hat{\\omega} + \\theta)   a⋅cos(\\theta)   a⋅cos(\\Delta{t}⋅\\hat{\\omeg\n",
       "───────────────────────────────── - ───────────── + ──────────────────────────\n",
       "        \\hat{\\omega}                            2                           2 \n",
       "                                    \\hat{\\omega}                \\hat{\\omega}  \n",
       "                                                                              \n",
       "s(\\Delta{t}⋅\\hat{\\omega} + \\theta)   a⋅sin(\\theta)   a⋅sin(\\Delta{t}⋅\\hat{\\ome\n",
       "────────────────────────────────── - ───────────── + ─────────────────────────\n",
       "         \\hat{\\omega}                            2                           2\n",
       "                                     \\hat{\\omega}                \\hat{\\omega} \n",
       "                                                                              \n",
       "⋅\\hat{\\omega} + \\theta                                                        \n",
       "                                                                              \n",
       "Delta{t}⋅a + v                                                                \n",
       "                                                                              \n",
       " \\hat{\\omega}                                                                 \n",
       "                                                                              \n",
       "      a                                                                       \n",
       "\n",
       "a} + \\theta) ⎤\n",
       "──────────── ⎥\n",
       "             ⎥\n",
       "             ⎥\n",
       "             ⎥\n",
       "ga} + \\theta)⎥\n",
       "─────────────⎥\n",
       "             ⎥\n",
       "             ⎥\n",
       "             ⎥\n",
       "             ⎥\n",
       "             ⎥\n",
       "             ⎥\n",
       "             ⎥\n",
       "             ⎥\n",
       "             ⎥\n",
       "             ⎦"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "text/plain": [
       "'Plugging in the numbers:'"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "image/png": "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\n",
      "text/latex": [
       "$\\displaystyle \\left\\{ \\Delta{t} : 0.1, \\  \\hat{\\omega} : 2, \\  \\omega_{0} : 0, \\  \\theta : 0.5, \\  a : 2, \\  v : 2, \\  x : 42, \\  y : 23\\right\\}$"
      ],
      "text/plain": [
       "{\\Delta{t}: 0.1, \\hat{\\omega}: 2, \\omega₀: 0, \\theta: 0.5, a: 2, v: 2, x: 42, \n",
       "y: 23}"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\left[\\begin{matrix}42.1728437300543\\\\23.1186522301942\\\\0.7\\\\2.2\\\\2.0\\\\2.0\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "⎡42.1728437300543⎤\n",
       "⎢                ⎥\n",
       "⎢23.1186522301942⎥\n",
       "⎢                ⎥\n",
       "⎢      0.7       ⎥\n",
       "⎢                ⎥\n",
       "⎢      2.2       ⎥\n",
       "⎢                ⎥\n",
       "⎢      2.0       ⎥\n",
       "⎢                ⎥\n",
       "⎣      2.0       ⎦"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Jacobian of the next state with respect to the previous state:\n"
     ]
    },
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\left[\\begin{matrix}1 & 0 & \\frac{\\Delta{t} a \\cos{\\left(\\Delta{t} \\hat{\\omega} + \\theta \\right)}}{\\hat{\\omega}} - \\frac{v \\cos{\\left(\\theta \\right)}}{\\hat{\\omega}} + \\frac{v \\cos{\\left(\\Delta{t} \\hat{\\omega} + \\theta \\right)}}{\\hat{\\omega}} + \\frac{a \\sin{\\left(\\theta \\right)}}{\\hat{\\omega}^{2}} - \\frac{a \\sin{\\left(\\Delta{t} \\hat{\\omega} + \\theta \\right)}}{\\hat{\\omega}^{2}} & - \\frac{\\sin{\\left(\\theta \\right)}}{\\hat{\\omega}} + \\frac{\\sin{\\left(\\Delta{t} \\hat{\\omega} + \\theta \\right)}}{\\hat{\\omega}} & \\frac{\\Delta{t}^{2} a \\cos{\\left(\\Delta{t} \\hat{\\omega} + \\theta \\right)}}{\\hat{\\omega}} + \\frac{\\Delta{t} v \\cos{\\left(\\Delta{t} \\hat{\\omega} + \\theta \\right)}}{\\hat{\\omega}} - \\frac{2 \\Delta{t} a \\sin{\\left(\\Delta{t} \\hat{\\omega} + \\theta \\right)}}{\\hat{\\omega}^{2}} + \\frac{v \\sin{\\left(\\theta \\right)}}{\\hat{\\omega}^{2}} - \\frac{v \\sin{\\left(\\Delta{t} \\hat{\\omega} + \\theta \\right)}}{\\hat{\\omega}^{2}} + \\frac{2 a \\cos{\\left(\\theta \\right)}}{\\hat{\\omega}^{3}} - \\frac{2 a \\cos{\\left(\\Delta{t} \\hat{\\omega} + \\theta \\right)}}{\\hat{\\omega}^{3}} & \\frac{\\Delta{t} \\sin{\\left(\\Delta{t} \\hat{\\omega} + \\theta \\right)}}{\\hat{\\omega}} - \\frac{\\cos{\\left(\\theta \\right)}}{\\hat{\\omega}^{2}} + \\frac{\\cos{\\left(\\Delta{t} \\hat{\\omega} + \\theta \\right)}}{\\hat{\\omega}^{2}}\\\\0 & 1 & \\frac{\\Delta{t} a \\sin{\\left(\\Delta{t} \\hat{\\omega} + \\theta \\right)}}{\\hat{\\omega}} - \\frac{v \\sin{\\left(\\theta \\right)}}{\\hat{\\omega}} + \\frac{v \\sin{\\left(\\Delta{t} \\hat{\\omega} + \\theta \\right)}}{\\hat{\\omega}} - \\frac{a \\cos{\\left(\\theta \\right)}}{\\hat{\\omega}^{2}} + \\frac{a \\cos{\\left(\\Delta{t} \\hat{\\omega} + \\theta \\right)}}{\\hat{\\omega}^{2}} & \\frac{\\cos{\\left(\\theta \\right)}}{\\hat{\\omega}} - \\frac{\\cos{\\left(\\Delta{t} \\hat{\\omega} + \\theta \\right)}}{\\hat{\\omega}} & \\frac{\\Delta{t}^{2} a \\sin{\\left(\\Delta{t} \\hat{\\omega} + \\theta \\right)}}{\\hat{\\omega}} + \\frac{\\Delta{t} v \\sin{\\left(\\Delta{t} \\hat{\\omega} + \\theta \\right)}}{\\hat{\\omega}} + \\frac{2 \\Delta{t} a \\cos{\\left(\\Delta{t} \\hat{\\omega} + \\theta \\right)}}{\\hat{\\omega}^{2}} - \\frac{v \\cos{\\left(\\theta \\right)}}{\\hat{\\omega}^{2}} + \\frac{v \\cos{\\left(\\Delta{t} \\hat{\\omega} + \\theta \\right)}}{\\hat{\\omega}^{2}} + \\frac{2 a \\sin{\\left(\\theta \\right)}}{\\hat{\\omega}^{3}} - \\frac{2 a \\sin{\\left(\\Delta{t} \\hat{\\omega} + \\theta \\right)}}{\\hat{\\omega}^{3}} & - \\frac{\\Delta{t} \\cos{\\left(\\Delta{t} \\hat{\\omega} + \\theta \\right)}}{\\hat{\\omega}} - \\frac{\\sin{\\left(\\theta \\right)}}{\\hat{\\omega}^{2}} + \\frac{\\sin{\\left(\\Delta{t} \\hat{\\omega} + \\theta \\right)}}{\\hat{\\omega}^{2}}\\\\0 & 0 & 1 & 0 & \\Delta{t} & 0\\\\0 & 0 & 0 & 1 & 0 & \\Delta{t}\\\\0 & 0 & 0 & 0 & 1 & 0\\\\0 & 0 & 0 & 0 & 0 & 1\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "⎡                                                                             \n",
       "⎢      \\Delta{t}⋅a⋅cos(\\Delta{t}⋅\\hat{\\omega} + \\theta)   v⋅cos(\\theta)   v⋅co\n",
       "⎢1  0  ──────────────────────────────────────────────── - ───────────── + ────\n",
       "⎢                        \\hat{\\omega}                      \\hat{\\omega}       \n",
       "⎢                                                                             \n",
       "⎢                                                                             \n",
       "⎢                                                                             \n",
       "⎢      \\Delta{t}⋅a⋅sin(\\Delta{t}⋅\\hat{\\omega} + \\theta)   v⋅sin(\\theta)   v⋅si\n",
       "⎢0  1  ──────────────────────────────────────────────── - ───────────── + ────\n",
       "⎢                        \\hat{\\omega}                      \\hat{\\omega}       \n",
       "⎢                                                                             \n",
       "⎢                                                                             \n",
       "⎢0  0                                                                         \n",
       "⎢                                                                             \n",
       "⎢0  0                                                                         \n",
       "⎢                                                                             \n",
       "⎢0  0                                                                         \n",
       "⎢                                                                             \n",
       "⎣0  0                                                                         \n",
       "\n",
       "                                                                              \n",
       "s(\\Delta{t}⋅\\hat{\\omega} + \\theta)   a⋅sin(\\theta)   a⋅sin(\\Delta{t}⋅\\hat{\\ome\n",
       "────────────────────────────────── + ───────────── - ─────────────────────────\n",
       "         \\hat{\\omega}                            2                           2\n",
       "                                     \\hat{\\omega}                \\hat{\\omega} \n",
       "                                                                              \n",
       "                                                                              \n",
       "n(\\Delta{t}⋅\\hat{\\omega} + \\theta)   a⋅cos(\\theta)   a⋅cos(\\Delta{t}⋅\\hat{\\ome\n",
       "────────────────────────────────── - ───────────── + ─────────────────────────\n",
       "         \\hat{\\omega}                            2                           2\n",
       "                                     \\hat{\\omega}                \\hat{\\omega} \n",
       "                                                                              \n",
       "         1                                                                    \n",
       "                                                                              \n",
       "         0                                                                    \n",
       "                                                                              \n",
       "         0                                                                    \n",
       "                                                                              \n",
       "         0                                                                    \n",
       "\n",
       "                                                                              \n",
       "ga} + \\theta)    sin(\\theta)    sin(\\Delta{t}⋅\\hat{\\omega} + \\theta)  \\Delta{t\n",
       "─────────────  - ──────────── + ────────────────────────────────────  ────────\n",
       "                 \\hat{\\omega}               \\hat{\\omega}                      \n",
       "                                                                              \n",
       "                                                                              \n",
       "                                                                              \n",
       "ga} + \\theta)   cos(\\theta)    cos(\\Delta{t}⋅\\hat{\\omega} + \\theta)   \\Delta{t\n",
       "─────────────   ──────────── - ────────────────────────────────────   ────────\n",
       "                \\hat{\\omega}               \\hat{\\omega}                       \n",
       "                                                                              \n",
       "                                                                              \n",
       "                                         0                                    \n",
       "                                                                              \n",
       "                                         1                                    \n",
       "                                                                              \n",
       "                                         0                                    \n",
       "                                                                              \n",
       "                                         0                                    \n",
       "\n",
       " 2                                                                            \n",
       "} ⋅a⋅cos(\\Delta{t}⋅\\hat{\\omega} + \\theta)   \\Delta{t}⋅v⋅cos(\\Delta{t}⋅\\hat{\\om\n",
       "───────────────────────────────────────── + ──────────────────────────────────\n",
       "           \\hat{\\omega}                                       \\hat{\\omega}    \n",
       "                                                                              \n",
       "                                                                              \n",
       " 2                                                                            \n",
       "} ⋅a⋅sin(\\Delta{t}⋅\\hat{\\omega} + \\theta)   \\Delta{t}⋅v⋅sin(\\Delta{t}⋅\\hat{\\om\n",
       "───────────────────────────────────────── + ──────────────────────────────────\n",
       "           \\hat{\\omega}                                       \\hat{\\omega}    \n",
       "                                                                              \n",
       "                                                                              \n",
       "                                                                              \n",
       "                                                                              \n",
       "                                                                              \n",
       "                                                                              \n",
       "                                                                              \n",
       "                                                                              \n",
       "                                                                              \n",
       "\n",
       "                                                                              \n",
       "ega} + \\theta)   2⋅\\Delta{t}⋅a⋅sin(\\Delta{t}⋅\\hat{\\omega} + \\theta)   v⋅sin(\\t\n",
       "────────────── - ────────────────────────────────────────────────── + ────────\n",
       "                                               2                              \n",
       "                                   \\hat{\\omega}                       \\hat{\\om\n",
       "                                                                              \n",
       "                                                                              \n",
       "ega} + \\theta)   2⋅\\Delta{t}⋅a⋅cos(\\Delta{t}⋅\\hat{\\omega} + \\theta)   v⋅cos(\\t\n",
       "────────────── + ────────────────────────────────────────────────── - ────────\n",
       "                                               2                              \n",
       "                                   \\hat{\\omega}                       \\hat{\\om\n",
       "                                                                              \n",
       "                                             \\Delta{t}                        \n",
       "                                                                              \n",
       "                                                 0                            \n",
       "                                                                              \n",
       "                                                 1                            \n",
       "                                                                              \n",
       "                                                 0                            \n",
       "\n",
       "                                                                              \n",
       "heta)   v⋅sin(\\Delta{t}⋅\\hat{\\omega} + \\theta)   2⋅a⋅cos(\\theta)   2⋅a⋅cos(\\De\n",
       "───── - ────────────────────────────────────── + ─────────────── - ───────────\n",
       "    2                           2                             3               \n",
       "ega}                \\hat{\\omega}                  \\hat{\\omega}                \n",
       "                                                                              \n",
       "                                                                              \n",
       "heta)   v⋅cos(\\Delta{t}⋅\\hat{\\omega} + \\theta)   2⋅a⋅sin(\\theta)   2⋅a⋅sin(\\De\n",
       "───── + ────────────────────────────────────── + ─────────────── - ───────────\n",
       "    2                           2                             3               \n",
       "ega}                \\hat{\\omega}                  \\hat{\\omega}                \n",
       "                                                                              \n",
       "                                                                              \n",
       "                                                                              \n",
       "                                                                              \n",
       "                                                                              \n",
       "                                                                              \n",
       "                                                                              \n",
       "                                                                              \n",
       "\n",
       "                                                                              \n",
       "lta{t}⋅\\hat{\\omega} + \\theta)   \\Delta{t}⋅sin(\\Delta{t}⋅\\hat{\\omega} + \\theta)\n",
       "─────────────────────────────   ──────────────────────────────────────────────\n",
       "              3                                  \\hat{\\omega}                 \n",
       "  \\hat{\\omega}                                                                \n",
       "                                                                              \n",
       "                                                                              \n",
       "lta{t}⋅\\hat{\\omega} + \\theta)    \\Delta{t}⋅cos(\\Delta{t}⋅\\hat{\\omega} + \\theta\n",
       "─────────────────────────────  - ─────────────────────────────────────────────\n",
       "              3                                   \\hat{\\omega}                \n",
       "  \\hat{\\omega}                                                                \n",
       "                                                                              \n",
       "                                                                              \n",
       "                                                                              \n",
       "                                                                              \n",
       "                                                                              \n",
       "                                                                              \n",
       "                                                                              \n",
       "                                                                              \n",
       "\n",
       "                                                        ⎤\n",
       "    cos(\\theta)    cos(\\Delta{t}⋅\\hat{\\omega} + \\theta) ⎥\n",
       " - ───────────── + ──────────────────────────────────── ⎥\n",
       "               2                          2             ⎥\n",
       "   \\hat{\\omega}               \\hat{\\omega}              ⎥\n",
       "                                                        ⎥\n",
       "                                                        ⎥\n",
       ")    sin(\\theta)    sin(\\Delta{t}⋅\\hat{\\omega} + \\theta)⎥\n",
       "─ - ───────────── + ────────────────────────────────────⎥\n",
       "                2                          2            ⎥\n",
       "    \\hat{\\omega}               \\hat{\\omega}             ⎥\n",
       "                                                        ⎥\n",
       "    0                                                   ⎥\n",
       "                                                        ⎥\n",
       "\\Delta{t}                                               ⎥\n",
       "                                                        ⎥\n",
       "    0                                                   ⎥\n",
       "                                                        ⎥\n",
       "    1                                                   ⎦"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "text/plain": [
       "'Plugging in the numbers:'"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "image/png": "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\n",
      "text/latex": [
       "$\\displaystyle \\left\\{ \\Delta{t} : 0.1, \\  \\hat{\\omega} : 2, \\  \\omega_{0} : 0, \\  \\theta : 0.5, \\  a : 2, \\  v : 2, \\  x : 42, \\  y : 23\\right\\}$"
      ],
      "text/plain": [
       "{\\Delta{t}: 0.1, \\hat{\\omega}: 2, \\omega₀: 0, \\theta: 0.5, a: 2, v: 2, x: 42, \n",
       "y: 23}"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\left[\\begin{matrix}1.0 & 0 & -0.118652230194179 & 0.082396074316744 & -0.00631501513627726 & 0.00402579071041348\\\\0 & 1.0 & 0.172843730054315 & 0.0563701873029421 & 0.00858190270490869 & 0.00295592779414759\\\\0 & 0 & 1.0 & 0 & 0.1 & 0\\\\0 & 0 & 0 & 1.0 & 0 & 0.1\\\\0 & 0 & 0 & 0 & 1.0 & 0\\\\0 & 0 & 0 & 0 & 0 & 1.0\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "⎡1.0   0   -0.118652230194179  0.082396074316744   -0.00631501513627726  0.004\n",
       "⎢                                                                             \n",
       "⎢ 0   1.0  0.172843730054315   0.0563701873029421  0.00858190270490869   0.002\n",
       "⎢                                                                             \n",
       "⎢ 0    0          1.0                  0                   0.1                \n",
       "⎢                                                                             \n",
       "⎢ 0    0           0                  1.0                   0                 \n",
       "⎢                                                                             \n",
       "⎢ 0    0           0                   0                   1.0                \n",
       "⎢                                                                             \n",
       "⎣ 0    0           0                   0                    0                 \n",
       "\n",
       "02579071041348⎤\n",
       "              ⎥\n",
       "95592779414759⎥\n",
       "              ⎥\n",
       "    0         ⎥\n",
       "              ⎥\n",
       "   0.1        ⎥\n",
       "              ⎥\n",
       "    0         ⎥\n",
       "              ⎥\n",
       "   1.0        ⎦"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Assuming a zero turn rate, state is:\n"
     ]
    },
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\left[\\begin{matrix}\\frac{\\Delta{t}^{2} a \\cos{\\left(\\Delta{t} \\omega_{0} + \\theta \\right)}}{2} + \\Delta{t} v \\cos{\\left(\\Delta{t} \\omega_{0} + \\theta \\right)} + x\\\\\\frac{\\Delta{t}^{2} a \\sin{\\left(\\Delta{t} \\omega_{0} + \\theta \\right)}}{2} + \\Delta{t} v \\sin{\\left(\\Delta{t} \\omega_{0} + \\theta \\right)} + y\\\\\\theta\\\\\\Delta{t} a + v\\\\\\omega_{0}\\\\a\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "⎡         2                                                                   \n",
       "⎢\\Delta{t} ⋅a⋅cos(\\Delta{t}⋅\\omega₀ + \\theta)                                 \n",
       "⎢──────────────────────────────────────────── + \\Delta{t}⋅v⋅cos(\\Delta{t}⋅\\ome\n",
       "⎢                     2                                                       \n",
       "⎢                                                                             \n",
       "⎢         2                                                                   \n",
       "⎢\\Delta{t} ⋅a⋅sin(\\Delta{t}⋅\\omega₀ + \\theta)                                 \n",
       "⎢──────────────────────────────────────────── + \\Delta{t}⋅v⋅sin(\\Delta{t}⋅\\ome\n",
       "⎢                     2                                                       \n",
       "⎢                                                                             \n",
       "⎢                                            \\theta                           \n",
       "⎢                                                                             \n",
       "⎢                                       \\Delta{t}⋅a + v                       \n",
       "⎢                                                                             \n",
       "⎢                                           \\omega₀                           \n",
       "⎢                                                                             \n",
       "⎣                                              a                              \n",
       "\n",
       "                 ⎤\n",
       "                 ⎥\n",
       "ga₀ + \\theta) + x⎥\n",
       "                 ⎥\n",
       "                 ⎥\n",
       "                 ⎥\n",
       "                 ⎥\n",
       "ga₀ + \\theta) + y⎥\n",
       "                 ⎥\n",
       "                 ⎥\n",
       "                 ⎥\n",
       "                 ⎥\n",
       "                 ⎥\n",
       "                 ⎥\n",
       "                 ⎥\n",
       "                 ⎥\n",
       "                 ⎦"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "text/plain": [
       "'Plugging in the numbers:'"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "image/png": "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\n",
      "text/latex": [
       "$\\displaystyle \\left\\{ \\Delta{t} : 0.1, \\  \\hat{\\omega} : 2, \\  \\omega_{0} : 0, \\  \\theta : 0.5, \\  a : 2, \\  v : 2, \\  x : 42, \\  y : 23\\right\\}$"
      ],
      "text/plain": [
       "{\\Delta{t}: 0.1, \\hat{\\omega}: 2, \\omega₀: 0, \\theta: 0.5, a: 2, v: 2, x: 42, \n",
       "y: 23}"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\left[\\begin{matrix}42.184292337997\\\\23.1006793631069\\\\0.5\\\\2.2\\\\0\\\\2.0\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "⎡42.184292337997 ⎤\n",
       "⎢                ⎥\n",
       "⎢23.1006793631069⎥\n",
       "⎢                ⎥\n",
       "⎢      0.5       ⎥\n",
       "⎢                ⎥\n",
       "⎢      2.2       ⎥\n",
       "⎢                ⎥\n",
       "⎢       0        ⎥\n",
       "⎢                ⎥\n",
       "⎣      2.0       ⎦"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Jacobian of the next state with respect to the previous state with 0 turn rate:\n"
     ]
    },
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\left[\\begin{matrix}1 & 0 & - \\frac{\\Delta{t}^{2} a \\sin{\\left(\\Delta{t} \\omega_{0} + \\theta \\right)}}{2} - \\Delta{t} v \\sin{\\left(\\Delta{t} \\omega_{0} + \\theta \\right)} & \\Delta{t} \\cos{\\left(\\Delta{t} \\omega_{0} + \\theta \\right)} & - \\frac{\\Delta{t}^{3} a \\sin{\\left(\\Delta{t} \\omega_{0} + \\theta \\right)}}{2} - \\Delta{t}^{2} v \\sin{\\left(\\Delta{t} \\omega_{0} + \\theta \\right)} & \\frac{\\Delta{t}^{2} \\cos{\\left(\\Delta{t} \\omega_{0} + \\theta \\right)}}{2}\\\\0 & 1 & \\frac{\\Delta{t}^{2} a \\cos{\\left(\\Delta{t} \\omega_{0} + \\theta \\right)}}{2} + \\Delta{t} v \\cos{\\left(\\Delta{t} \\omega_{0} + \\theta \\right)} & \\Delta{t} \\sin{\\left(\\Delta{t} \\omega_{0} + \\theta \\right)} & \\frac{\\Delta{t}^{3} a \\cos{\\left(\\Delta{t} \\omega_{0} + \\theta \\right)}}{2} + \\Delta{t}^{2} v \\cos{\\left(\\Delta{t} \\omega_{0} + \\theta \\right)} & \\frac{\\Delta{t}^{2} \\sin{\\left(\\Delta{t} \\omega_{0} + \\theta \\right)}}{2}\\\\0 & 0 & 1 & 0 & 0 & 0\\\\0 & 0 & 0 & 1 & 0 & \\Delta{t}\\\\0 & 0 & 0 & 0 & 1 & 0\\\\0 & 0 & 0 & 0 & 0 & 1\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "⎡                 2                                                           \n",
       "⎢        \\Delta{t} ⋅a⋅sin(\\Delta{t}⋅\\omega₀ + \\theta)                         \n",
       "⎢1  0  - ──────────────────────────────────────────── - \\Delta{t}⋅v⋅sin(\\Delta\n",
       "⎢                             2                                               \n",
       "⎢                                                                             \n",
       "⎢                2                                                            \n",
       "⎢       \\Delta{t} ⋅a⋅cos(\\Delta{t}⋅\\omega₀ + \\theta)                          \n",
       "⎢0  1   ──────────────────────────────────────────── + \\Delta{t}⋅v⋅cos(\\Delta{\n",
       "⎢                            2                                                \n",
       "⎢                                                                             \n",
       "⎢0  0                                               1                         \n",
       "⎢                                                                             \n",
       "⎢0  0                                               0                         \n",
       "⎢                                                                             \n",
       "⎢0  0                                               0                         \n",
       "⎢                                                                             \n",
       "⎣0  0                                               0                         \n",
       "\n",
       "                                                                             3\n",
       "                                                                    \\Delta{t} \n",
       "{t}⋅\\omega₀ + \\theta)  \\Delta{t}⋅cos(\\Delta{t}⋅\\omega₀ + \\theta)  - ──────────\n",
       "                                                                              \n",
       "                                                                              \n",
       "                                                                            3 \n",
       "                                                                   \\Delta{t} ⋅\n",
       "t}⋅\\omega₀ + \\theta)   \\Delta{t}⋅sin(\\Delta{t}⋅\\omega₀ + \\theta)   ───────────\n",
       "                                                                              \n",
       "                                                                              \n",
       "                                           0                                  \n",
       "                                                                              \n",
       "                                           1                                  \n",
       "                                                                              \n",
       "                                           0                                  \n",
       "                                                                              \n",
       "                                           0                                  \n",
       "\n",
       "                                                                              \n",
       "⋅a⋅sin(\\Delta{t}⋅\\omega₀ + \\theta)            2                               \n",
       "────────────────────────────────── - \\Delta{t} ⋅v⋅sin(\\Delta{t}⋅\\omega₀ + \\the\n",
       "           2                                                                  \n",
       "                                                                              \n",
       "                                                                              \n",
       "a⋅cos(\\Delta{t}⋅\\omega₀ + \\theta)            2                                \n",
       "───────────────────────────────── + \\Delta{t} ⋅v⋅cos(\\Delta{t}⋅\\omega₀ + \\thet\n",
       "          2                                                                   \n",
       "                                                                              \n",
       "                                  0                                           \n",
       "                                                                              \n",
       "                                  0                                           \n",
       "                                                                              \n",
       "                                  1                                           \n",
       "                                                                              \n",
       "                                  0                                           \n",
       "\n",
       "              2                                ⎤\n",
       "     \\Delta{t} ⋅cos(\\Delta{t}⋅\\omega₀ + \\theta)⎥\n",
       "ta)  ──────────────────────────────────────────⎥\n",
       "                         2                     ⎥\n",
       "                                               ⎥\n",
       "              2                                ⎥\n",
       "     \\Delta{t} ⋅sin(\\Delta{t}⋅\\omega₀ + \\theta)⎥\n",
       "a)   ──────────────────────────────────────────⎥\n",
       "                         2                     ⎥\n",
       "                                               ⎥\n",
       "                         0                     ⎥\n",
       "                                               ⎥\n",
       "                     \\Delta{t}                 ⎥\n",
       "                                               ⎥\n",
       "                         0                     ⎥\n",
       "                                               ⎥\n",
       "                         1                     ⎦"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "text/plain": [
       "'Plugging in the numbers:'"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "image/png": "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\n",
      "text/latex": [
       "$\\displaystyle \\left\\{ \\Delta{t} : 0.1, \\  \\hat{\\omega} : 2, \\  \\omega_{0} : 0, \\  \\theta : 0.5, \\  a : 2, \\  v : 2, \\  x : 42, \\  y : 23\\right\\}$"
      ],
      "text/plain": [
       "{\\Delta{t}: 0.1, \\hat{\\omega}: 2, \\omega₀: 0, \\theta: 0.5, a: 2, v: 2, x: 42, \n",
       "y: 23}"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\left[\\begin{matrix}1.0 & 0 & -0.100679363106883 & 0.0877582561890373 & -0.0100679363106883 & 0.00438791280945186\\\\0 & 1.0 & 0.184292337996978 & 0.0479425538604203 & 0.0184292337996978 & 0.00239712769302102\\\\0 & 0 & 1.0 & 0 & 0 & 0\\\\0 & 0 & 0 & 1.0 & 0 & 0.1\\\\0 & 0 & 0 & 0 & 1.0 & 0\\\\0 & 0 & 0 & 0 & 0 & 1.0\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "⎡1.0   0   -0.100679363106883  0.0877582561890373  -0.0100679363106883  0.0043\n",
       "⎢                                                                             \n",
       "⎢ 0   1.0  0.184292337996978   0.0479425538604203  0.0184292337996978   0.0023\n",
       "⎢                                                                             \n",
       "⎢ 0    0          1.0                  0                    0                 \n",
       "⎢                                                                             \n",
       "⎢ 0    0           0                  1.0                   0                 \n",
       "⎢                                                                             \n",
       "⎢ 0    0           0                   0                   1.0                \n",
       "⎢                                                                             \n",
       "⎣ 0    0           0                   0                    0                 \n",
       "\n",
       "8791280945186⎤\n",
       "             ⎥\n",
       "9712769302102⎥\n",
       "             ⎥\n",
       "   0         ⎥\n",
       "             ⎥\n",
       "  0.1        ⎥\n",
       "             ⎥\n",
       "   0         ⎥\n",
       "             ⎥\n",
       "  1.0        ⎦"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "speed, yaw, dt, x, y, acceleration, t, d_sin, d_cos = symbols('v \\\\theta \\Delta{t} x y a t d_{sin} d_{cos}')\n",
    "turn_rate_non_zero = Symbol('\\hat{\\omega}', nonzero=True, finite=True)\n",
    "zero_turn_rate = Symbol('\\omega_0', zero=True)\n",
    "turn_rate = Symbol('\\omega')\n",
    "\n",
    "state_catr = Matrix([x, y, yaw, speed, turn_rate, acceleration])\n",
    "print(\"CART state is:\")\n",
    "display(state_catr)\n",
    "\n",
    "def get_next_state(turn_rate):\n",
    "    # Specify the functions for yaw, speed, x, and y.\n",
    "    yaw_func = yaw + turn_rate * t\n",
    "    speed_func = speed + acceleration * t\n",
    "    x_speed_func = speed_func * cos(yaw_func)\n",
    "    y_speed_func = speed_func * sin(yaw_func)\n",
    "\n",
    "    # Get next state by integrating the functions.\n",
    "    next_speed = speed + integrate(acceleration, (t, 0, dt))\n",
    "    next_yaw = yaw + integrate(turn_rate, (t, 0, dt))\n",
    "    next_x = x + integrate(x_speed_func, (t, 0, dt))\n",
    "    next_y = y + integrate(y_speed_func, (t, 0, dt))\n",
    "\n",
    "    return Matrix([next_x, next_y, next_yaw, next_speed, turn_rate, acceleration])\n",
    "\n",
    "# There is a difference in computation betwee the cases when the turn rate is allowed to be zero or not\n",
    "print(\"Assuming a non-zero turn rate, the next state is:\")\n",
    "next_state = get_next_state(turn_rate_non_zero)\n",
    "display(next_state)\n",
    "\n",
    "substitutes = {x:42, y:23, yaw:0.5, speed:2, acceleration:2, dt:0.1, turn_rate_non_zero:2, zero_turn_rate:0}\n",
    "display('Plugging in the numbers:', substitutes)\n",
    "display(next_state.evalf(subs=substitutes))\n",
    "\n",
    "state = Matrix([x,y,yaw,speed,turn_rate_non_zero,acceleration])\n",
    "print(\"Jacobian of the next state with respect to the previous state:\")\n",
    "J = next_state.jacobian(state)\n",
    "display(J)\n",
    "display('Plugging in the numbers:', substitutes)\n",
    "display(J.evalf(subs=substitutes))\n",
    "\n",
    "print(\"Assuming a zero turn rate, state is:\")\n",
    "next_state = get_next_state(zero_turn_rate)\n",
    "display(next_state)\n",
    "display('Plugging in the numbers:', substitutes)\n",
    "display(next_state.evalf(subs=substitutes))\n",
    "\n",
    "state = Matrix([x,y,yaw,speed,zero_turn_rate,acceleration])\n",
    "print(\"Jacobian of the next state with respect to the previous state with 0 turn rate:\")\n",
    "J = next_state.jacobian(state)\n",
    "display(J)\n",
    "\n",
    "display('Plugging in the numbers:', substitutes)\n",
    "display(J.evalf(subs=substitutes))\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## CVTR - constant velocity and turn rate model\n",
    "This model assumes the state $[x, y, \\theta, v, \\omega]^\\top$, where $x$ and $y$ are coordinates of the object, $\\theta$ is it CCW orientation, $v$ is its speed along the vector spawned by $\\theta$, $\\omega$ is the angular velocity (aka turn rate). Furthermore, we assume that the object has constant speed $v$ and constant turn rate $\\omega$. This is also a useful model to model differential-drive systems. It is a bit simpler than the CATR one.\n",
    "\n",
    "### Derivation:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "CVRT state is:\n"
     ]
    },
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\left[\\begin{matrix}x\\\\y\\\\\\theta\\\\v\\\\\\omega\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "⎡  x   ⎤\n",
       "⎢      ⎥\n",
       "⎢  y   ⎥\n",
       "⎢      ⎥\n",
       "⎢\\theta⎥\n",
       "⎢      ⎥\n",
       "⎢  v   ⎥\n",
       "⎢      ⎥\n",
       "⎣\\omega⎦"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Assuming a non-zero turn rate, next state is:\n"
     ]
    },
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\left[\\begin{matrix}x - \\frac{v \\sin{\\left(\\theta \\right)}}{\\omega} + \\frac{v \\sin{\\left(\\Delta{t} \\omega + \\theta \\right)}}{\\omega}\\\\y + \\frac{v \\cos{\\left(\\theta \\right)}}{\\omega} - \\frac{v \\cos{\\left(\\Delta{t} \\omega + \\theta \\right)}}{\\omega}\\\\\\Delta{t} \\omega + \\theta\\\\v\\\\\\omega\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "⎡    v⋅sin(\\theta)   v⋅sin(\\Delta{t}⋅\\omega + \\theta)⎤\n",
       "⎢x - ───────────── + ────────────────────────────────⎥\n",
       "⎢        \\omega                   \\omega             ⎥\n",
       "⎢                                                    ⎥\n",
       "⎢    v⋅cos(\\theta)   v⋅cos(\\Delta{t}⋅\\omega + \\theta)⎥\n",
       "⎢y + ───────────── - ────────────────────────────────⎥\n",
       "⎢        \\omega                   \\omega             ⎥\n",
       "⎢                                                    ⎥\n",
       "⎢             \\Delta{t}⋅\\omega + \\theta              ⎥\n",
       "⎢                                                    ⎥\n",
       "⎢                         v                          ⎥\n",
       "⎢                                                    ⎥\n",
       "⎣                       \\omega                       ⎦"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "text/markdown": [
       "Plugging in the numbers:"
      ],
      "text/plain": [
       "<IPython.core.display.Markdown object>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "image/png": "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\n",
      "text/latex": [
       "$\\displaystyle \\left\\{ \\Delta{t} : 0.1, \\  \\omega : 2, \\  \\omega_{0} : 0, \\  \\theta : 0.5, \\  v : 2, \\  x : 42, \\  y : 23\\right\\}$"
      ],
      "text/plain": [
       "{\\Delta{t}: 0.1, \\omega: 2, \\omega₀: 0, \\theta: 0.5, v: 2, x: 42, y: 23}"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\left[\\begin{matrix}42.1647921486335\\\\23.1127403746059\\\\0.7\\\\2.0\\\\2.0\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "⎡42.1647921486335⎤\n",
       "⎢                ⎥\n",
       "⎢23.1127403746059⎥\n",
       "⎢                ⎥\n",
       "⎢      0.7       ⎥\n",
       "⎢                ⎥\n",
       "⎢      2.0       ⎥\n",
       "⎢                ⎥\n",
       "⎣      2.0       ⎦"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Jacobian of the next state with respect to the previous state:\n"
     ]
    },
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\left[\\begin{matrix}1 & 0 & - \\frac{v \\cos{\\left(\\theta \\right)}}{\\omega} + \\frac{v \\cos{\\left(\\Delta{t} \\omega + \\theta \\right)}}{\\omega} & - \\frac{\\sin{\\left(\\theta \\right)}}{\\omega} + \\frac{\\sin{\\left(\\Delta{t} \\omega + \\theta \\right)}}{\\omega} & \\frac{\\Delta{t} v \\cos{\\left(\\Delta{t} \\omega + \\theta \\right)}}{\\omega} + \\frac{v \\sin{\\left(\\theta \\right)}}{\\omega^{2}} - \\frac{v \\sin{\\left(\\Delta{t} \\omega + \\theta \\right)}}{\\omega^{2}}\\\\0 & 1 & - \\frac{v \\sin{\\left(\\theta \\right)}}{\\omega} + \\frac{v \\sin{\\left(\\Delta{t} \\omega + \\theta \\right)}}{\\omega} & \\frac{\\cos{\\left(\\theta \\right)}}{\\omega} - \\frac{\\cos{\\left(\\Delta{t} \\omega + \\theta \\right)}}{\\omega} & \\frac{\\Delta{t} v \\sin{\\left(\\Delta{t} \\omega + \\theta \\right)}}{\\omega} - \\frac{v \\cos{\\left(\\theta \\right)}}{\\omega^{2}} + \\frac{v \\cos{\\left(\\Delta{t} \\omega + \\theta \\right)}}{\\omega^{2}}\\\\0 & 0 & 1 & 0 & \\Delta{t}\\\\0 & 0 & 0 & 1 & 0\\\\0 & 0 & 0 & 0 & 1\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "⎡        v⋅cos(\\theta)   v⋅cos(\\Delta{t}⋅\\omega + \\theta)    sin(\\theta)   sin\n",
       "⎢1  0  - ───────────── + ────────────────────────────────  - ─────────── + ───\n",
       "⎢            \\omega                   \\omega                    \\omega        \n",
       "⎢                                                                             \n",
       "⎢                                                                             \n",
       "⎢        v⋅sin(\\theta)   v⋅sin(\\Delta{t}⋅\\omega + \\theta)   cos(\\theta)   cos(\n",
       "⎢0  1  - ───────────── + ────────────────────────────────   ─────────── - ────\n",
       "⎢            \\omega                   \\omega                   \\omega         \n",
       "⎢                                                                             \n",
       "⎢                                                                             \n",
       "⎢0  0                          1                                              \n",
       "⎢                                                                             \n",
       "⎢0  0                          0                                              \n",
       "⎢                                                                             \n",
       "⎣0  0                          0                                              \n",
       "\n",
       "(\\Delta{t}⋅\\omega + \\theta)  \\Delta{t}⋅v⋅cos(\\Delta{t}⋅\\omega + \\theta)   v⋅si\n",
       "───────────────────────────  ────────────────────────────────────────── + ────\n",
       "         \\omega                                \\omega                         \n",
       "                                                                             \\\n",
       "                                                                              \n",
       "\\Delta{t}⋅\\omega + \\theta)   \\Delta{t}⋅v⋅sin(\\Delta{t}⋅\\omega + \\theta)   v⋅co\n",
       "──────────────────────────   ────────────────────────────────────────── - ────\n",
       "        \\omega                                 \\omega                         \n",
       "                                                                             \\\n",
       "                                                                              \n",
       "   0                                                                   \\Delta{\n",
       "                                                                              \n",
       "   1                                                                       0  \n",
       "                                                                              \n",
       "   0                                                                       1  \n",
       "\n",
       "n(\\theta)   v⋅sin(\\Delta{t}⋅\\omega + \\theta)⎤\n",
       "───────── - ────────────────────────────────⎥\n",
       "     2                        2             ⎥\n",
       "omega                   \\omega              ⎥\n",
       "                                            ⎥\n",
       "s(\\theta)   v⋅cos(\\Delta{t}⋅\\omega + \\theta)⎥\n",
       "───────── + ────────────────────────────────⎥\n",
       "     2                        2             ⎥\n",
       "omega                   \\omega              ⎥\n",
       "                                            ⎥\n",
       "t}                                          ⎥\n",
       "                                            ⎥\n",
       "                                            ⎥\n",
       "                                            ⎥\n",
       "                                            ⎦"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "text/markdown": [
       "Plugging in the numbers:"
      ],
      "text/plain": [
       "<IPython.core.display.Markdown object>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "image/png": "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\n",
      "text/latex": [
       "$\\displaystyle \\left\\{ \\Delta{t} : 0.1, \\  \\omega : 2, \\  \\omega_{0} : 0, \\  \\theta : 0.5, \\  v : 2, \\  x : 42, \\  y : 23\\right\\}$"
      ],
      "text/plain": [
       "{\\Delta{t}: 0.1, \\omega: 2, \\omega₀: 0, \\theta: 0.5, v: 2, x: 42, y: 23}"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\left[\\begin{matrix}1.0 & 0 & -0.112740374605884 & 0.082396074316744 & -0.00591185558829518\\\\0 & 1.0 & 0.164792148633488 & 0.0563701873029421 & 0.00805158142082696\\\\0 & 0 & 1.0 & 0 & 0.1\\\\0 & 0 & 0 & 1.0 & 0\\\\0 & 0 & 0 & 0 & 1.0\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "⎡1.0   0   -0.112740374605884  0.082396074316744   -0.00591185558829518⎤\n",
       "⎢                                                                      ⎥\n",
       "⎢ 0   1.0  0.164792148633488   0.0563701873029421  0.00805158142082696 ⎥\n",
       "⎢                                                                      ⎥\n",
       "⎢ 0    0          1.0                  0                   0.1         ⎥\n",
       "⎢                                                                      ⎥\n",
       "⎢ 0    0           0                  1.0                   0          ⎥\n",
       "⎢                                                                      ⎥\n",
       "⎣ 0    0           0                   0                   1.0         ⎦"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Assuming a zero turn rate, next state is:\n"
     ]
    },
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\left[\\begin{matrix}\\Delta{t} v \\cos{\\left(\\Delta{t} \\omega_{0} + \\theta \\right)} + x\\\\\\Delta{t} v \\sin{\\left(\\Delta{t} \\omega_{0} + \\theta \\right)} + y\\\\\\theta\\\\v\\\\\\omega_{0}\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "⎡\\Delta{t}⋅v⋅cos(\\Delta{t}⋅\\omega₀ + \\theta) + x⎤\n",
       "⎢                                               ⎥\n",
       "⎢\\Delta{t}⋅v⋅sin(\\Delta{t}⋅\\omega₀ + \\theta) + y⎥\n",
       "⎢                                               ⎥\n",
       "⎢                    \\theta                     ⎥\n",
       "⎢                                               ⎥\n",
       "⎢                       v                       ⎥\n",
       "⎢                                               ⎥\n",
       "⎣                    \\omega₀                    ⎦"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "text/markdown": [
       "Plugging in the numbers:"
      ],
      "text/plain": [
       "<IPython.core.display.Markdown object>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "image/png": "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\n",
      "text/latex": [
       "$\\displaystyle \\left\\{ \\Delta{t} : 0.1, \\  \\omega : 2, \\  \\omega_{0} : 0, \\  \\theta : 0.5, \\  v : 2, \\  x : 42, \\  y : 23\\right\\}$"
      ],
      "text/plain": [
       "{\\Delta{t}: 0.1, \\omega: 2, \\omega₀: 0, \\theta: 0.5, v: 2, x: 42, y: 23}"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\left[\\begin{matrix}42.1755165123781\\\\23.0958851077208\\\\0.5\\\\2.0\\\\0\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "⎡42.1755165123781⎤\n",
       "⎢                ⎥\n",
       "⎢23.0958851077208⎥\n",
       "⎢                ⎥\n",
       "⎢      0.5       ⎥\n",
       "⎢                ⎥\n",
       "⎢      2.0       ⎥\n",
       "⎢                ⎥\n",
       "⎣       0        ⎦"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Jacobian of the next state with respect to the previous one with zero turn rate:\n"
     ]
    },
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\left[\\begin{matrix}1 & 0 & - \\Delta{t} v \\sin{\\left(\\Delta{t} \\omega_{0} + \\theta \\right)} & \\Delta{t} \\cos{\\left(\\Delta{t} \\omega_{0} + \\theta \\right)} & - \\Delta{t}^{2} v \\sin{\\left(\\Delta{t} \\omega_{0} + \\theta \\right)}\\\\0 & 1 & \\Delta{t} v \\cos{\\left(\\Delta{t} \\omega_{0} + \\theta \\right)} & \\Delta{t} \\sin{\\left(\\Delta{t} \\omega_{0} + \\theta \\right)} & \\Delta{t}^{2} v \\cos{\\left(\\Delta{t} \\omega_{0} + \\theta \\right)}\\\\0 & 0 & 1 & 0 & 0\\\\0 & 0 & 0 & 1 & 0\\\\0 & 0 & 0 & 0 & 1\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "⎡                                                                             \n",
       "⎢1  0  -\\Delta{t}⋅v⋅sin(\\Delta{t}⋅\\omega₀ + \\theta)  \\Delta{t}⋅cos(\\Delta{t}⋅\\\n",
       "⎢                                                                             \n",
       "⎢                                                                             \n",
       "⎢0  1  \\Delta{t}⋅v⋅cos(\\Delta{t}⋅\\omega₀ + \\theta)   \\Delta{t}⋅sin(\\Delta{t}⋅\\\n",
       "⎢                                                                             \n",
       "⎢0  0                       1                                            0    \n",
       "⎢                                                                             \n",
       "⎢0  0                       0                                            1    \n",
       "⎢                                                                             \n",
       "⎣0  0                       0                                            0    \n",
       "\n",
       "                            2                                  ⎤\n",
       "omega₀ + \\theta)  -\\Delta{t} ⋅v⋅sin(\\Delta{t}⋅\\omega₀ + \\theta)⎥\n",
       "                                                               ⎥\n",
       "                           2                                   ⎥\n",
       "omega₀ + \\theta)  \\Delta{t} ⋅v⋅cos(\\Delta{t}⋅\\omega₀ + \\theta) ⎥\n",
       "                                                               ⎥\n",
       "                                        0                      ⎥\n",
       "                                                               ⎥\n",
       "                                        0                      ⎥\n",
       "                                                               ⎥\n",
       "                                        1                      ⎦"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "text/markdown": [
       "Plugging in the numbers:"
      ],
      "text/plain": [
       "<IPython.core.display.Markdown object>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "image/png": "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\n",
      "text/latex": [
       "$\\displaystyle \\left\\{ \\Delta{t} : 0.1, \\  \\omega : 2, \\  \\omega_{0} : 0, \\  \\theta : 0.5, \\  v : 2, \\  x : 42, \\  y : 23\\right\\}$"
      ],
      "text/plain": [
       "{\\Delta{t}: 0.1, \\omega: 2, \\omega₀: 0, \\theta: 0.5, v: 2, x: 42, y: 23}"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\left[\\begin{matrix}1.0 & 0 & -0.0958851077208406 & 0.0877582561890373 & -0.00958851077208406\\\\0 & 1.0 & 0.175516512378075 & 0.0479425538604203 & 0.0175516512378075\\\\0 & 0 & 1.0 & 0 & 0\\\\0 & 0 & 0 & 1.0 & 0\\\\0 & 0 & 0 & 0 & 1.0\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "⎡1.0   0   -0.0958851077208406  0.0877582561890373  -0.00958851077208406⎤\n",
       "⎢                                                                       ⎥\n",
       "⎢ 0   1.0   0.175516512378075   0.0479425538604203   0.0175516512378075 ⎥\n",
       "⎢                                                                       ⎥\n",
       "⎢ 0    0           1.0                  0                    0          ⎥\n",
       "⎢                                                                       ⎥\n",
       "⎢ 0    0            0                  1.0                   0          ⎥\n",
       "⎢                                                                       ⎥\n",
       "⎣ 0    0            0                   0                   1.0         ⎦"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "speed, yaw, dt, x, y, t = symbols('v \\\\theta \\Delta{t} x y t')\n",
    "turn_rate_non_zero = Symbol('\\omega', nonzero=True, finite=True)\n",
    "zero_turn_rate = Symbol('\\omega_0', zero=True)\n",
    "\n",
    "state_cvtr = Matrix([x, y, yaw, speed, turn_rate_non_zero])\n",
    "print(\"CVRT state is:\")\n",
    "display(state_cvtr)\n",
    "\n",
    "def get_next_state(turn_rate):\n",
    "    # Specify the functions for yaw, x, and y.\n",
    "    yaw_func = yaw + turn_rate * t\n",
    "    x_speed_func = speed * cos(yaw_func)\n",
    "    y_speed_func = speed * sin(yaw_func)\n",
    "\n",
    "    # Get next state by integrating the functions.\n",
    "    next_yaw = yaw + integrate(turn_rate, (t, 0, dt))\n",
    "    next_x = x + integrate(x_speed_func, (t, 0, dt))\n",
    "    next_y = y + integrate(y_speed_func, (t, 0, dt))\n",
    "\n",
    "    return Matrix([next_x, next_y, next_yaw, speed, turn_rate])\n",
    "\n",
    "# There is a difference in computation betwee the cases when the turn rate is allowed to be zero or not\n",
    "print(\"Assuming a non-zero turn rate, next state is:\")\n",
    "next_state = get_next_state(turn_rate_non_zero)\n",
    "display(next_state)\n",
    "substitutes = {x:42, y:23, yaw:0.5, speed:2, dt:0.1, turn_rate_non_zero:2, zero_turn_rate:0}\n",
    "display(Markdown('Plugging in the numbers:'), substitutes)\n",
    "display(next_state.evalf(subs=substitutes))\n",
    "\n",
    "state = Matrix([x,y,yaw,speed,turn_rate_non_zero])\n",
    "print(\"Jacobian of the next state with respect to the previous state:\")\n",
    "J = next_state.jacobian(state)\n",
    "display(J)\n",
    "\n",
    "display(Markdown('Plugging in the numbers:'), substitutes)\n",
    "display(J.evalf(subs=substitutes))\n",
    "\n",
    "print(\"Assuming a zero turn rate, next state is:\")\n",
    "next_state = get_next_state(zero_turn_rate)\n",
    "display(next_state)\n",
    "display(Markdown('Plugging in the numbers:'), substitutes)\n",
    "display(next_state.evalf(subs=substitutes))\n",
    "\n",
    "state = Matrix([x,y,yaw,speed,zero_turn_rate])\n",
    "print(\"Jacobian of the next state with respect to the previous one with zero turn rate:\")\n",
    "J = next_state.jacobian(state)\n",
    "display(J)\n",
    "\n",
    "display(Markdown('Plugging in the numbers:'), substitutes)\n",
    "display(J.evalf(subs=substitutes))"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.8.5"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}
